// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-

/*
ArduPilotMegaJeti Version 1.0 Public Beta
Author:     Uwe Gartmann

Authors of ArduPilotMega:	Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Michael Smith
Thanks to:	Chris Anderson, HappyKillMore, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi

This firmware is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
*/

/// Definitions

/// Includes
#define HardwareSerial_h	// Disable Arduino serial library
#include <Common.h>			// Some common definitions
#include <BoardIO.h>		// Digital IO
#include <mySerial0.h>		// Console
#include <mySerial1.h>		// GPS serial port
#include <JetiModel.h>		// Serial 3
#include <ADS7844.h>		// ADC ->> Gyro, Acc, Temp
#include <BMP085.h>			// Pressure Sensor
#include <HMC5843.h>		// 3D Compass
#include <UBLOX.h>			// GPS library
#include <RadioControl.h>	// Radio Control Library


// AVR runtime
#include <Wire.h>
//#include <avr/io.h>
#include <avr/eeprom.h>
//#include <avr/pgmspace.h>
#include <math.h>

// 
#include "definitions.h"

// Configuration
#include "settings.h"


/// Declarations
BoardIO			IO;
mySerial0		Serial0;	// Console
mySerial1		Serial1;	// GPS
JetiModel		Jeti;		// Telemetry
ADS7844			IMU;
BMP085			Altimeter;
HMC5843			Compass;
UBLOX			GPS(&Serial1);
RadioControl	RC;
LoopTimer		LT;

long loopCntr = 0;

// Basic Initialization
//---------------------
void setup() {
	initIOD();
	initJeti();
	initGPS();
	initRC();
	LT.Init();	// synchronize timer once again
}

void loop() {
	// Check for SwitchMode
	if (IO.SlideSwitchMode() == SLIDE_SW_FLIGHT) {

		if (LT.Timer50Hz()) {
			updateGPS(); // GPS data is updated 4 times a second. updateGPS() checks for new data itself.
			updateRC();
			updateIMU();
			
			// do Navigation
			// get Control
			// set RC servos
		}

		if (LT.Timer20Hz()) {
			updateIOD(); // Compass, Altimeter
			updateAHRS();
		}
		
		if (LT.Timer10Hz()) {
			updateJeti();			
			IO.LED(LED_GREEN, TOGGLE); // Heartbeat;
		}

		if (LT.Timer5Hz()) {
		}

		if (LT.Timer2Hz()) {
		}

		if (LT.Timer1Hz()) {
		}
		
		if (LT.Timer10Sec()) {
		}

	} else { // none flight, interactive mode
		
		if (LT.Timer10Hz()) {
			IO.LED(LED_RED, TOGGLE); // Heartbeat;
		}
	}
}

